#include <rclcpp/rclcpp.hpp> 
#include "power_board_pkg/base_driver.h"
#include <stdint.h>
#include "power_board_pkg/timer_ros.h"
#include <boost/thread.hpp>
#include <power_board_pkg/simple_dataframe_master.h>

bool g_StopThread = false;

rclcpp::Node::SharedPtr node_ptr;
/* tcp监听线程 */
void Tcp_Read_Thread(void)
{
    RCLCPP_INFO(node_ptr->get_logger(),"create tcp recvice thread succeed!\n");
    rclcpp::Rate loop_rate(1000);
    
    while(g_StopThread == false)
    {
	    Simple_dataframe::Frame_Instance()->recv_proc();
        loop_rate.sleep();
    }
    RCLCPP_INFO(node_ptr->get_logger(),"stop tcp recvice thread!\n");
}

void Shut_Down_Thread(void)
{
    RCLCPP_INFO(node_ptr->get_logger(),"create shutdown thread succeed!\n");
    rclcpp::Rate loop_rate(1000);

    while(g_StopThread == false)
    {
        if(Data_holder::get()->Flag._shutdownflag == true || BaseDriver::Instance()->Power_state.data == "LIMIT")       //关机按钮按下或者极限电量情况下自动关机
        {
            RCLCPP_INFO(node_ptr->get_logger(),"wait to shutdown...\n");
            rclcpp::sleep_for(std::chrono::seconds(4));
            Simple_dataframe::Frame_Instance()->interact(SET_POWER_CLOSE,NULL,0);
            RCLCPP_INFO(node_ptr->get_logger(),"shutdown...\n");
            system("sudo sync");
            system("sudo shutdown -h now");
        }
        // else if(BaseDriver::Instance()->Power_state.data == "LIMIT")  //极限电量 yyy 2020-10-31 v3.2
        // {
        //     // std_msgs::msg::String str;
        //     // str.data = "LIMIT";
        //     // BaseDriver::Instance()->power_state_pub->publish(str);
        //     RCLCPP_INFO(node_ptr->get_logger(),"wait to shutdown...\n");
        //     rclcpp::Duration(4).sleep();
        //     Simple_dataframe::Frame_Instance()->interact(SET_POWER_CLOSE,NULL,0);
        //     RCLCPP_INFO(node_ptr->get_logger(),"shutdown...\n");
        //     system("sudo sync");
        //     system("sudo shutdown -h now");
        // }
        // if((Data_holder::get()->batteryinfo.soc <= BaseDriver::Instance()->g_Limit_Power &&
        //     Data_holder::get()->Flag._batteryupdate == true))
        // {
        //     RCLCPP_INFO(node_ptr->get_logger(),"BaseDriver::Instance()->g_Limit_Power = %d\n",BaseDriver::Instance()->g_Limit_Power);
        //     RCLCPP_INFO(node_ptr->get_logger(),"Data_holder::get()->batteryinfo.soc = %d\n",Data_holder::get()->batteryinfo.soc);
        // }
        loop_rate.sleep();
    }

    // Data_holder* dh = Data_holder::get();
   
    RCLCPP_INFO(node_ptr->get_logger(),"stop tcp recvice thread!\n");
}

int main(int argc, char *argv[])
{

    rclcpp::init(argc, argv);
    node_ptr = std::make_shared<rclcpp::Node>("power_board_node");
    std::string test = " ";

    // 创建ROS2节点对象并进行初始化    
    // 初始化子订阅      
    BaseDriver::set_static_node(node_ptr);
    Tcp_Talk::set_static_node(node_ptr);
    Simple_dataframe::set_static_node(node_ptr); 


    node_ptr->declare_parameter("/power_board/limitpower",3);
    BaseDriver::Instance()->g_Limit_Power = node_ptr->get_parameter("/power_board/limitpower").as_int();

    node_ptr->declare_parameter("charge_floor","20");
    test = node_ptr->get_parameter("charge_floor").as_string();
    BaseDriver::Instance()->g_Lower_Power = atoi(test.c_str());


	RCLCPP_INFO(node_ptr->get_logger(),"POWER PARAMETERS:\n\t\t\t\tlimitpower = %d\n\t\t\t\tllowerpower = %d\n",BaseDriver::Instance()->g_Limit_Power,BaseDriver::Instance()->g_Lower_Power);
    
    BaseDriver::Instance()->init_ros_topic();
    
    
    do
    {
        if(Tcp_Talk::Tcp_Instance()->Tcp_Client_Init() < 0)
        {
            continue;
        }
        else
        {
            break;
        }
    }
    while(1);
    
    boost::thread tcpread_thread(Tcp_Read_Thread);
    boost::thread shutdown_thread(Shut_Down_Thread);
    rclcpp::sleep_for(std::chrono::milliseconds(500));
	BaseDriver::Instance()->work_loop();
    g_StopThread = true;
    
    tcpread_thread.join();
    shutdown_thread.join();
    
  
    rclcpp::spin(node_ptr);  
    // 关闭ROS2 C++接口
    rclcpp::shutdown(); 
    return 0;
}



